// Локальные
#include "linal/los.h"
#include "linal/gauss.h"

namespace linal
{
    //============================================================================================================================
    //============================================================================================================================
    double LOS::solveLU(const CSRMatrix &matrix, const Vector &f, Vector &x)
    {
        double alpha, beta;     //Вспомогательные константы метода.
        double norm_f;          //Норма вектора правой части (Чтобы не пересчитывать много раз).

        // Получение факторизованной матрицы
        CSRMatrix factored_matrix = matrix.partFactorLU();

        //вычисление нормы f
        norm_f = f.normSquared();

        //нахождение r нуле-вого
        m_R = matrix*x;
        m_R = f - m_R;
        Gauss::solveLowSLAE(factored_matrix, m_R);

        //нахождение z нулевого
        m_Z = m_R;
        Gauss::solveUpSLAE(factored_matrix, m_Z);

        //нахождение p нуле-вого
        m_P = matrix*m_Z;
        Gauss::solveLowSLAE(factored_matrix, m_P);

        //итерационный шаг
        double residual = std::numeric_limits<double>::max();
        for(int cur_iter = 0; cur_iter < m_MaxNumOfIter && residual > m_SufficientResidual*m_SufficientResidual; cur_iter++)
        {
            double pp_scalar = m_P*m_P; //Норма вектора p в квадрате (Чтобы не вычислять два раза).
            //нахождение alpha i-го
            alpha = (m_P*m_R)/pp_scalar;
            if(alpha < 1e-10)
                break;

            //нахождение x i-го
            x += alpha*m_Z;

            //нахождение r i-го
            m_R -= alpha*m_P;

            //вспомогательный вектор для нахождения beta
            m_Temp1 = m_R;
            Gauss::solveUpSLAE(factored_matrix, m_Temp1);
            m_Temp2 = matrix*m_Temp1;
            Gauss::solveLowSLAE(factored_matrix, m_Temp2);

            //нахождение beta i-го
            beta = -(m_P*m_Temp2)/(pp_scalar);

            //нахождение p i-го
            m_P *= beta;
            m_P += m_Temp2;

            //вспомогательный вектор для нахождения z i-го
            m_Temp1 = m_R;
            Gauss::solveUpSLAE(factored_matrix, m_Temp1);

            //нахождение z i-го
            m_Z *= beta;
            m_Z += m_Temp1;

            //нахождение невязки
            residual = m_R.normSquared()/norm_f;
        }
        return qSqrt(residual);
    }

    //============================================================================================================================
    //============================================================================================================================
    double LOS::solveDi(const CSRMatrix &matrix, const Vector &f, Vector &x)
    {
        double alpha, beta;     //Вспомогательные константы метода.
        double norm_f;          //Норма вектора правой части (Чтобы не пересчитывать много раз).

        //вычисление нормы f
        norm_f = f.normSquared();

        //нахождение r нуле-вого
        m_R = matrix*x;
        m_R = f - m_R;
        for(int i = 0; i < m_R.dimension(); ++i)
            m_R[i] /= matrix(i, i);

        //нахождение z нулевого
        m_Z = m_R;

        //нахождение p нуле-вого
        m_P = matrix*m_Z;
        for(int i = 0; i < m_P.dimension(); ++i)
            m_P[i] /= matrix(i, i);

        //итерационный шаг
        double residual = std::numeric_limits<double>::max();
        for(int cur_iter = 0; cur_iter < m_MaxNumOfIter && residual > m_SufficientResidual*m_SufficientResidual; cur_iter++)
        {
            double pp_scalar = m_P*m_P; //Норма вектора p в квадрате (Чтобы не вычислять два раза).
            //нахождение alpha i-го
            alpha = (m_P*m_R)/pp_scalar;
            if(alpha < 1e-10)
                break;

            //нахождение x i-го
            x += alpha*m_Z;

            //нахождение r i-го
            m_R -= alpha*m_P;

            //вспомогательный вектор для нахождения beta
            m_Temp1 = matrix*m_R;
            for(int i = 0; i < m_Temp1.dimension(); ++i)
                m_Temp1[i] /= matrix(i, i);

            //нахождение beta i-го
            beta = -(m_P*m_Temp1)/(pp_scalar);

            //нахождение p i-го
            m_P *= beta;
            m_P += m_Temp1;

            //нахождение z i-го
            m_Z *= beta;
            m_Z += m_R;

            //нахождение невязки
            residual = m_R.normSquared()/norm_f;
        }
        return qSqrt(residual);
    }

    //============================================================================================================================
    //============================================================================================================================
    double LOS::solve(const CSRMatrix &matrix, const Vector &f, Vector &x)
    {
        double alpha, beta;     //Вспомогательные константы метода.
        double norm_f;          //Норма вектора правой части (Чтобы не пересчитывать много раз).

        //вычисление нормы f
        norm_f = f.normSquared();

        //нахождение r нуле-вого
        m_R = matrix*x;
        m_R = f - m_R;

        //нахождение z нулевого
        m_Z = m_R;

        //нахождение p нуле-вого
        m_P = matrix*m_Z;

        //итерационный шаг
        double residual = std::numeric_limits<double>::max();
        for(int cur_iter = 0; cur_iter < m_MaxNumOfIter && residual > m_SufficientResidual*m_SufficientResidual; cur_iter++)
        {
            double pp_scalar = m_P*m_P; //Норма вектора p в квадрате (Чтобы не вычислять два раза).
            //нахождение alpha i-го
            alpha = (m_P*m_R)/pp_scalar;
            if(alpha < 1e-10)
                break;

            //нахождение x i-го
            x += alpha*m_Z;

            //нахождение r i-го
            m_R -= alpha*m_P;

            //вспомогательный вектор для нахождения beta
            m_Temp1 = matrix*m_R;

            //нахождение beta i-го
            beta = -(m_P*m_Temp1)/(pp_scalar);

            //нахождение p i-го
            m_P *= beta;
            m_P += m_Temp1;

            //нахождение z i-го
            m_Z *= beta;
            m_Z += m_R;

            //нахождение невязки
            residual = m_R.normSquared()/norm_f;
        }
        return qSqrt(residual);
    }
}
